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a INTRODUCTION 

Autonomous vehicles that are suitable for vse in both 
naval and commercial operations, have unique mission 
requirements and dynamic characteristics. In particular, they 
are required to be highly maneuverable and very responsive as 
they operate in obstacle avoidance and object recognition 
tasks. The need therefore, arises to maintain accurate path 
keeping in confined spaces under the influence of steady and 
time varying external excitation. The primary vehicle 
guidance system is based on heading or turning rate commands 
that are generated based on a specified geographical sequence 
of desired way points. These guidance commands are then 
passed to the vehicle controller which attempts to deliver the 
commanded heading and/or heading rate of change by an 
appropriate use of the vehicle control surfaces [Ref.1]. For 
vehicle operations in confined spaces the way point sequence 
must be very dense so that satisfactory path accuracy is 
maintained. 

One efficient way of maneuvering through a given way point 
sequence is by using a line of sight guidance law which 
commands a heading angle that is directly related to the line 
of sight angle between the vehicle position and a desired 
destination point. The vehicle controller is then an 


orientation control law which delivers the commanded heading. 


Previous studies [Ref.2], have demonstrated that this 
scheme is guaranteed stable only if the way point separation 
is above some critical value. This conclusion is true 
regardless of the particular form of the line of sight 
guidance or the heading control law used. In this work we 
analyze the turning rate guidance and control problem where 
the guidance law demands a specific yaw rate response from the 
controller. A linear state feedback with a feedforward term 
[Ref.3], control law is used, while two different guidance 
schemes are considered. The first is a cross track error 
guidance law which is very popular in land based robotic 
applications [Ref.4]. The second is a proportional guidance 
law which is based on the line of sight angle between the 
vehicle and a target point. Stability analysis is performed 
and numerical integrations are used to confirm the theoretical 
results. All numerical computations in this work are 
performed for the Naval Postgratuate School autonomous 
vehicle, for which a complete set of geometric properties and 


hydrodynamic characteristics is available [Ref.5]. 


II. EQUATIONS OF MOTION 
Restricting our attention to the horizontal plane, the 
mathematical model consists GE the nonlinear sway 
(translational motion parallel to the vehicle longitudinal 
axis) and yaw (rotational motion about the vertical axis) 
equations of motion. In a moving coordinate frame fixed at 
the vehicle’s geometrical center (see Fig.l), Newton’s 


equations of motion are: 


MVA TL SY (1) 


I,r+mx.(d+ur)=N, (2) 


where m is the mass of the vehicle , I, its moment of inertia 
with respect to the vertical axis, u the forward velocity, x, 
the coordinate of the vehicle center of gravity with respect 
to its centroid, V and r are the relative sway and yaw 
velocities of the moving vehicle with respect to the water; 
and Y, N represent the total excitation sway force and yaw 
moment, respectively. 

Following standard vehicle maneuvering assumptions, these 
forces can be expressed as the sum of quadratic drag terms and 
first order polynomials in v and r with constant coefficients. 
In this way the nonlinear equations of motion in the 


horizontal plane become 


M(V+UF+X E] EY, I+Y VA FUF Peele: 
+Yu?6-[Ch(E) (v+Er) [u+Er|dE (3) 


T,L+mx,(b+ur) =N,I+N,V+N,ur+N uv+N,u?d- 


-0.5@[C5h(E) (u+Er) [u+Erj&dE (4) 


In equation 3 the terms Y;, Y, represent the change in the 
lateral force due to unit changes in the angular acceleration 
r and the lateral translational acceleration v, respectively. 
Likewise, the terms Y,, Y, represent the change in the lateral 
force due to unit changes in the corresponding velocities v 
and r. The terms N;, Ni, N,, N, in equation 4 are defined 
similarly. C, is the drag coefficient and 6 is the rudder 
angle. The NPS AUV II is equipped with both stern and bow 
rudder which are identical in size and are deflected in 


opposite directions for maximum maneuverability. In other 


words 
0.8, ö,=-8 (5) 


where 6, is the stern rudder angle, and 0, the bow rudder 
angle. The cross flow integral drag terms in the equations of 
motion become important for hovering operations or low speed 
maneuvering, whereas at high speeds u the steering response is 
predominantly linear. 

To complete the model, we need the expressions for the 


vehicle yaw rate 


WHr, (6) 
and the inertial position rates 


x=ucosY - vsinY , (7) 


y=usinY + vcosY , (8) 


where Ô, is the vehicle heading angle as shown in Figure 1. 
Although it is recognized that the steering gear is both rate 
and angle limited, steering gear dynamics are not included in 
the formulation since they are much faster than the dynamics 
of a turning vehicle. The methods presented in the following 
sections can easily accommodate such modifications if desired. 

The surge velocity u is clearly affected during the trim 
due to the added drag in turning. For the purposes of this 
study, it is assumed to be constant. This is a valid 
approximation since experimental experience has shown that the 
propulsion control law is in general capable of keeping the 


forward speed u relatively constant at the commanded value. 





Figure 1. Vehicle Geometry and Definitions of Symbols. 


ILL. CONTROL LAW DESIGN 


A. LINEARIZATION 

As was mentioned in the previous chapter, during regular 
cruising operations at about 2 ft/sec forward speed, the 
nonlinear term in the equations of motion 3 and 4 are small 
and, therefore, effective control can be maintained by using 


their linear form 
A Y Y + Y UY + Y USO. (9) 
I,1+mx,.(d+ur)=N,r+N,V+N.ur+N uv+N,u*ÿ. (10) 


After some algebra, equations 9 and 10 can be put into 


state space form as 


I UV d DUO, (11) 


<- 
| 


nn 2 
where 


DI NS ix) (N mx), 
Das ana N ms No) Ya 
eV inne) (menu) (Hm), 


Db, EEN VE (AK YIN, 


Db, = (m-Y,)N¿- (mx _-Nv) Y4 . 


D= (1,N,) (m-Y Im. Y meer 
Equations 11 and 12 describe the lateral dynamics of the 


vehicle for small motions. 


B. FEEDBACK CONTROL 
A linear rudder feedback control law based on equations 11 


and 12 has the form 
ON KR Ur in (13) 


where k,, k, are the feedback gains. By substituting equation 


13 into 11 and 12 we get the closed loop dynamics equations 
V = (a utb u klv (a on ES (14) 
r= (a,,utbju’k,) v+(a,,usb uk) 3 (15) 
The characteristic equation of 14 and 15 is 
A2+A,A+A, = O, (16) 
where 
A, = la,,+taz,+ (b,k,+b,k,)u] u, 


A,=[a,:4,,-2,,8:+ (ba ba uk (Dan Dres E 


Now the desired closed loop characteristic equation is 


Re OA RE O (17) 
By equating the coefficients of eqs.16 and 17 we get the 


following system of linear equations 


Ky U*+k,y u* = -q,-(a,,+a,,)u, (18) 


a O u tk 2D -a2 Blu? =a,- (19) 
- (&118227812821) U? 

Equations 18 and 19 then determine the gain k,, k,, in the 
feedback control law (Eq.13). 

The coefficients œ, QA, of the desired characteristic 
equation 17 can be specified according to any standard pole 
placement design technique. In this work we decide to use the 
controller time constant, T,, as the parameter that specifies 
the coefficients QA, 0). In this way the desired 


characteristic equation is 


(1+2)=0 Or fee PE (20) 
Te Te ne 
and comparing with eq.17 we see that 
2 1 
A, == , À, = — (21) 
To pe 


Specification of a controller time constant T, then 


determines the feedback gains k,, k, uniquely. 


C. FEEDFORWARD CONTROL 

The control law (Eq.13) guarantees stability of v=r=o0 
of equations 11 and 12, in other words straight line motion at 
an arbitrary heading. When the commanded angular velocity r., 


is nonzero the control law is slightly modified to 

ET VA nae (22) 
where r,, is the commanded turning rate and k,, is the 
feedforward gain. This is computed based on steady state 
accuracy requirements. At steady state, equations 11 and 12 
yield 

Vev ; 6 = 6 r (23) 
where 


_b,a,,-b,8,, Sere 4214127 411422 


c , (24) 
b,a,,7D,821 b,a,,70,87,U 


Substituting equations 23 and 24 into equation 23, and 
requiring that r = r, at steady state, we can solve for k,, 
kom O KE (25) 
and the control law (Eq.22) becomes 


O=K Vtk r-k 0 ck (49 


Substituting the values of k,, Ô., k,, V, in (26), we can 


cf 


finally write the control law (Eq.26) in the form 


10 


O» 
Ii 


> E (27) 


C , 


where 


1 
Ms Un 


Ko (28) 


With the above feed forward gain the control law is 
complete. It should be mentioned that all gains K,, k,, Ko 
depend explicitly on the forward speed u and are, therefore, 
continuously updated every time a different forward speed is 


commanded. 


D. RESULTS 

The response of the control law (Eq.27) applied to the 
system of Equations 11 and 12 is shown in Figure 2, where the 
commanded turning rate is 0.1 rad/sec. It can be seen that 
the actual vehicle turning rate r converges to its commanded 
value r,, as expected. The gain were computed based on a 
selected time constant T, = 1 dimensionless second. Time is 
nondimensionalized here with respect to the vehicle forward 
speed u = 2 ft/sec and the vehicle length L = 7.3 ft. 
Therefore, one dimensionless second corresponds to 3.65 
seconds of real time and this is verified by the simulation 
results of Figure 2 (the response drops about 60% every time 
constant). 

The response for different time constants (in 


dimensionless seconds) is shown in Figure 3 where it can be 


11 


seen that higher values of T,, result in slower vehicle 
response as expected. Similar is the rudder angle response as 
shown in Figure 4. Smaller values of T,, result in tighter 


control and more rudder activity. 


E. ACCURACY 

The feedforward gain k, computed from eq.28 ensures that 
the steady state turning rate r equals the commanded value r, 
for the linear system of equations 11 and 12. To analyze the 
effects of nonlinearities we start with the nonlinear 


equations of motion 3 and 4 and we write them in the form 


V=a,uvta,ur+bu7S+d,(v,r), (29) 

i = a, uUv+a Ur *D USAN ee (30) 
Dd. (v, r)=-O859'C, | (1,-N en) toe 
Dd;(v,r}==0 5eC, ms MO Ie 


D= (I ,=N,} (MEME NMXE PARC ee 


1,=[h(E) (u+Er) |v+Er|dE , 


1,=[h(E) (v+Er) [v+Er|EdE 


The values of v and r at steady state can then be computed 


from 
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40 


a jüuvtapurtb u ord EGER (31) 


a,,UV+a,,Ur+b,u°o+d_(v, 2) 0R (32) 
-kv krika r -0n (33) 
The solution to these equations is not r = r, unless d, = 


d, = 0. This is demonstrated by the time simulation of Figure 
5, for which the quadratic drag coefficient C, was set equal 
to 0.5. It can be seen that the vehicle turning rate develops 
a nonzero steady state error with respect to the commanded 
value of 0.1 rad/sec. As the controller time constant T, is 
decreased, the control law becomes tighter and this steady 
state error could not become zero due to uncertainties in the 
vehicle hydrodynamic description and various unmodeled 
dynamics. Even if k, were determined from equations 31 to 33 


such that r = fr the uncertainties involved in the 


er 
determination of the hydrodynamic coefficients in the 
equations of motion would cause the actual value of r to be 
different than the commanded value r,. One way to ensure 
steady state accuracy is to abandon the use of the feedforward 


gain k, and to introduce integral control. This possibility 


is examined in the next section. 
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E. INTEGRAL CONTROL 
An alternative way to ensure stability and convergence to 
the commanded value r, is to introduce integral control in the 


form 
Ô=k,v+k,(r-r,) Ze de (34) 
In that case, the system of state equations is augmented 
by the additional 
PoS ToL. (35) 


In order to compute the gains k,, k,, k, we set r, = 0 and 


substitute the control law 
Òk vik rik r (36) 
into equations 11 and 12. The resulting closed loop system is 
v= (a, u+k, b, u*) v+ (a,u+k_b,u*) r+k,b, u*r, (37) 


38 
F= (a, uU+K bus) v+ (a UK OU es Dn a 


À, = (39) 


The characteristic equation of equations 37 to 39 can be 


computed as 
40 
A’+A,A’+a,\A+ta, = 0 E 


where 


18 


E a UD Ut buk. 


A,= (a,,8727 821912) us (a,,D,-a7,0,) Ue Ket 
x (a,b, - 24,0, } uk O cig, | 
A (ar be D) uk, , 


If the desired characteristic equation is 


à> +a, À? +a, À +a, =0 (41) 
by equating the coefficients of equations 40 and 41 we get the 
following system of linear equations which determines the 


three gains k,, k,, Kr: 


2 om = 


= 3 Es 3 — 2 
= = 2 
(21182 271412) u 


f 


kia nb -azb u a; 


For a time constant T, and an integrator time constant T}, 


the desired characteristic equation becomes 








c T; 
OL 
Ad+ (242) 24 + 2 )A+ > (0. 
me T? TiTe TT 
so that 
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With these choices the determination of the gain k,, k,, 
k,, is complete. A numerical simulation for r, = 0.1 rad/sec. 
and with C, = 0.5 is presented in Figure 6. The time 
constants are T, = 1 and T, assumed the three different values 
shown in the figure. It can be seen that despite the 
nonlinear drag terms, steady state accuracy is achieved. 
Since the response of systems compensated by integral control 
action tends to be more oscillatory, in this work we will use 
the previous feedforward control. As we show in the next 


chapter, steady state accuracy is guaranteed once the control 


law is combined with a suitable guidance. 
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IV. GUIDANCE LAW 


A. GENERAL 

The previously developed control law is able to deliver a 
vehicle commanded turning rate r,. In order to achieve path 
control to a commanded route in the horizontal plane, however, 
the commanded turning rate r, must be appropriately selected. 
This constitutes the guidance law design. Two such guidance 
laws are described in the following, cross track error and 
proportional guidance. Without loss of generality, we assume 
that the commanded path is a straight line. This is not a 
very restrictive assumption since every smooth path can be 
discretized into a series of straight line segments as 


accurately as desired. 


B. CROSS TRACK ERROR GUIDANCE 
The guidance law is based solely on kinematics, whereas 
vehicle dynamics are handled by the control law. Guidance law 


development based on 


y =r (42) 


y = usiny , (43) 


where r, is the commanded turning rate and the lateral 


c 


velocity v is assumed to be zero in equation 43. 


Cross track error guidance is achieved by 


22 


r. = Kayak y (44) 


C 
By substituting equation 44 into equation 42 we get the 


closed loop guidance system 
w= kywekyy , (45) 


i = US aa (46) 
The characteristic equation of equations 45 and 46 is 
obtained by making the small angle approximation siny = y, and 
is 
22 a = 
12-k,A-k,u=0. (47) 
If the desired characteristic equation is 
12+PB,A+B,=0, (48) 


the guidance law gains k,, k, are obtained by equating the 


Y 


coefficients of equations 47 and 48 


ky E PA , (49) 


eee ome (50) 
U 


y 


Analogously to the control law design, if the time control 
of the guidance law is selected to be T¿, then equations 49 


and 50 result in 


k= -Ż, (51) 
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_ il 
KØRER TER (52) 
Tau 





Selection of T then determines ki akp diren m 

Although this development followed the small angle 
approximation, it is not difficult to see that negative values 
of k,; and k, will guarantee stability of the nonlinear sysco 
of equations 45 and 46. The associated total energy of the 


system is 


Ep, Y) => Ņ?-k,u(1-cosy) i (53) 


which can be viewed as the sum of kinetic and potential 


energy. Using equation 45, this is written as 
E(w,y) => (kodik y) USOS UE (54) 


We note that E(w,y) provides a Liapunov function for equations 
45 and 46, since E(0,0)=0 at the unique equilibrium 
(Y, y)=(0,0) and E(Y,y)>0 for (W,y)#(0,0), because k,<0 


Moreover, we have 


p= AE, AY, dE, dy - dE y+ 8E; 55 
E dy dt dy dt re ae 


Evaluating the indicated partial derivatives we get 


dE 


TE (kY+k,y)k,-kyusinvy , (56) 
Ba (kW thy) ky | (57) 
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Substituting equations 45, 46, 56 and 57 into equation 55 


we find 


E=k,(kypt+kyy)? , (58) 
which is negative. Therefore, Liapunov’s theorem guarantees 


asymptotic stability of the nonlinear system (equations 45 and 


46). 


C. PROPORTIONAL GUIDANCE 

A typical orientation based guidance law is pursuit 
guidance which is accomplished as follows: The commanded 
vehicle heading angle yw. equals the line of sight angle 
between the vehicle and a target point D located ahead of the 
vehicle and a constant preview distance d on the nominal 


straight line path as shown in Figure 1. In other words 
ee tan ió (59) 


In a proportional guidance scheme a time constant T is 


incorporated in the above line of sight equation 


TY += tanto , (60) 


and we arrive at the proportional guidance which is used here 


for path control 


Del Elan. , (61) 


al 
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I tan? 
T 


al< 


(62) 


(63) 
and by comparing it to equations 48 through 50 we can see that 


it corresponds to a guidance characteristic equation with 


B, = 


le 


(64) 


eS = (65) 
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V. STABILITY 


A. SIMULATIONS 

In order to assess stability of the combined guidance and 
control law we proceed first by a series of numerical 
integrations. The simulations are based on the dynamical 
equations 3 and 4 and the kinematic equations 6 and 8. The 
rudder control law in the form of equation 27 is used, while 
the cross track error guidance (eq.44) is employed. Results 
are presented in terms of the lateral deviation y in ft versus 
time t in seconds. Figure 7 shows the vehicle response for 
control time constant T,=0.5 and guidance time constant T,=1. 
It can be seen that the response is stable, although it 
exhibits slow convergence characteristics to the commanded 
path y=0. To speed-up speed of response we could lower the 
guidance time constant T,. The results of the simulation for 
T.=0.5 and T,=0.5 are presented in Figure 8. It can be seen 
that the convergence is now faster although the path overshoot 
is higher. If we maintain the same guidance time constant 
T¿=0.5 and increase the control time constant T,=2, the vehicle 
is unstable as demonstrated by Figure 9. The response 
exhibits now oscillatory characteristics or a limit cycle. 
The explanation for this phenomenon is that, in this case the 


control law is very slow and the rudder cannot follow the 
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commanded turning rates by the guidance law which is 
considerably faster, due to its smaller time constant. To 
remedy this instability we should either lower T, as in Figure 
8 demonstrated, or increase the value of T,. The results of 
this simulation are shown in Figure 10, for which T,=2 and 
T¿=2. The response exhibits now characteristics of a damped 
oscillation and is stable, although convergence is very slow 


and oscillatory. 


B.: STABILITY 

The previous simulation results show that there exists a 
certain range of (T,, T.) combinations that results in stable 
response. In order to compute this range and verify the 
numerical integration results, we use the linearized equations 


Of motion 


Vez (66) 
v=a, uv+a,,ur+b,u*0, (67) 
F=a,,uv+a,,ur+b,u?b, (68) 
y=uy+v (69) 
the rudder control law 
Ó=k V+K_ I=k a (70) 


and the turning rate guidance law 


reek Y +K y. (71) 
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In a compact vector notation the linearized equations of 


motion 66 through 71 are written as 


A ave ey | (72) 

Motion stability is established by the eigenvalues of 
Matrix A, if all eigenvalues have negative real parts the 
nominal straight line motion is dynamically stable, while if 
at least one eigenvalue of A is positive, the system is 


unstable. Writing out the characteristic equation of equation 


72 we get 
A4+BA3+CA24+DA+E=0, (73) 
where 
B=a, , (74) 
C=a,-(b,ß,+b,uß,) uk,a, . (75) 
D=a,ß,+d,a,ß, . (76) 
E=a.P, . (77) 
and 


a b, 10,810 =D, 822 


d.= 
i (b,a,,-D,8,1) u 


(78) 
In the derivation of expressions 74 to 78 we have used 

equations 18 and 19 for the feedback gains, equation 28 for 

the feedforward gain, and equations 49 and 50 for the general 


guidance law gains. 
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If we apply Routh's criterion to the fourth order equation 
73 we can find the following critical condition for stability 
BCD Erz Drum (79) 


If we substitute equations 74 through 77 in equation 79, 


we find the following condition 


b 
a,a,[1-(—P,+b,B;) u?k,] (PB, +d,B2) 71,702 
ee SE 


(80) 


Equation 80 represents in algebraic form the critical 
condition for stability in terms of the control law 
characteristic equation coefficients Q,, œ and the guidance 
law coefficients P,, ß,. It can be used for both the cross 
track error and the proportional guidance schemes, if we 
employ the appropriate expressions for fP,, B, from Chapter IV. 

For the case of cross track error guidance, equation 80 


results in 


- 


T= 272 (b, t2b,. 2 a eee) (81) 
Amar 


For a given guidance law time constant T,, equation 81 
specifies the critical control time constant for stability. 

Stability of the combined guidance and control scheme is 
guaranteed for values of T, that are less than the critical 
value computed from equation 81. For T, values above the 
critical value (eq.80) the system is unstable. In these 


cases, one pair of complex conjugate eigenvalues of matrix A 
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in equation 72 has positive real parts, and as a result the 
response of the system is oscillatory. 

A plot of the critical stability condition (Eq.81) is 
shown in Figure de the response is stable for (T,, Tg) 
combinations that lie below the critical curve. The number 1 
through 4 on the figure correspond to the four numerical 
Simulations that were presented in Figure 7 through 10, 
respectively. It can be seen that the simulation results agree 
with the stability analysis performed in this section. Point 
3 is located above the critical curve and the results in the 


unstable oscillatory response observed in Figure 9. 


SL 


y (ft) 


Figure 7. 





time (sec) 


Path Keeping Response for T,=0.5, T,=1. 
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Figure 8. 
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time (sec) 


Path Keeping Response “ÉSE” T-=0N5), T¿*D025. 
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Figure 9. 
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Path Keeping Response for T,=2, T,=0.5. 
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Figure 10. 
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Path Keeping Response for T,=2, 
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Figure 11. Critical*"T,, T, Curve for stabality. 
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VI. CONCLUSIONS AND RECOMMENDATIONS 

The goal of this work, stability analysis of turning rate 
guidance and control for autonomous vehicles has been 
achieved. The control law was based on the dynamic equations 
in sway and yaw. The feedback gains were analytically 
computed by pole-placement techniques and the feedforward gain 
was evaluated based on the desired steady-state accuracy. 

The guidance laws that were utilized, cross-track error or 
proportional guidance, were based on the kinematics relations. 
It was found that unless the guidance and control laws were 
designed according to certain conditions, stability of the 
system was not guaranteed. These stability conditions were 
computed analytically and the results were verified by 
numerical simulations. Recommendations for future work 
include an analysis of the performance of turning-rate 
guidance and control from the point of view of sensitivity to 
sensor noise, state estimation, and disturbance rejection. 
Comparisons with other guidance and control laws, such as line 


of-sight guidance, should also be performed. 
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